#!/usr/bin/env python

import cv2.cv as cv
import numpy as np

def is_rect_nonzero(r):
    (_,_,w,h) = r
    return (w > 0) and (h > 0)

class fmTrackingDemo:

    def __init__(self):

        self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2, 2) 
        cv.NamedWindow( "fmTracking_Preview_Window", 1 )
        cv.NamedWindow( "fmTracking_Output_Window", 1 )
        cv.SetMouseCallback( "fmTracking_Preview_Window", self.on_mouse)

        self.__myStr=''
        self.__myTerm='\r'
        self.__myCmd=''
        self.__myState=0
        self.filename=''
 
        self.drag_start = None      # Set to (x,y) when mouse starts drag
        self.track_window = None    # Set to rect when the mouse drag finishes
        self.selection = (0,0,0,0)
        print( "Keys:\n"
            "    EXIT - quit the program\n"
            "    b - switch to/from backprojection view\n"
            "To initialize tracking, drag across the object with the mouse\n" )

    def set_capture_resolution(self, width, height):

        self.capture = cv.CaptureFromCAM(1)
        self.preview_scale = 0.5
        cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_HEIGHT, height)
        cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_WIDTH, width)
        cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FORMAT, cv.IPL_DEPTH_32F)

    def set_capture_file(self,filename):
        self.preview_scale = 1
        self.filename = filename

    def on_msgArrival(self,iStr):
        self.__myStr += iStr
        pos = self.__myStr.find(self.__myTerm)   
        while (pos >=0):
            self.on_Command(self.__myStr[0:pos])
            self.__myStr=self.__myStr[pos+1:len(self.__myStr)]
            pos =  self.__myStr.find(self.__myTerm)

    def on_Command(self,cmd):
        if len(cmd)>0 :
            print (":" + cmd)    
            if (cmd == 'exit'):
                self.__myState=-1;

    def on_mouse(self, event, x, y, flags, param):
        if event == cv.CV_EVENT_LBUTTONDOWN:
#            print x,y
            self.drag_start = (x, y)
        if event == cv.CV_EVENT_LBUTTONUP:
            self.drag_start = None
            self.track_window = self.selection
            if is_rect_nonzero(self.selection):
                print self.selection
        if self.drag_start:
            xmin = min(x, self.drag_start[0])
            ymin = min(y, self.drag_start[1])
            xmax = max(x, self.drag_start[0])
            ymax = max(y, self.drag_start[1])
            self.selection = (xmin, ymin, xmax - xmin, ymax - ymin)
 #           print self.selection
           
            
    def running_average(self, input_frame):
        output_frame = input_frame
        return output_frame

    def run(self):

        if (self.filename ==''):
            frame = cv.QueryFrame( self.capture )
        else:
            frame = cv.LoadImage(self.filename)
            
        size = cv.GetSize(frame)
        print (size)
        
        while True:
            
            # greb a frame

            if (self.filename ==''):
                frame = cv.QueryFrame( self.capture )

            # update preview window
            preview_frame = cv.CreateImage( ( int(size[0]*self.preview_scale) , int(size[1]*self.preview_scale)), frame.depth, frame.nChannels)
            cv.Resize(frame, preview_frame)
            cv.PutText(preview_frame,self.__myStr, (20,30),self.font,250) #Draw the text
#            cv.Circle(preview_frame,(100,100),50,(250,0,0),1,8)
#            cv.Rectangle(preview_frame,(100,100),(200,200),(250,0,0),1,8,0)
            if is_rect_nonzero(self.selection):
                x,y,w,h = self.selection
                cv.Rectangle(preview_frame,(x,y),(x+w,y+h),(250,0,0),1,8,0)

            cv.ShowImage( "fmTracking_Preview_Window", preview_frame)

            # Compute process frame
            if self.__myState==0:   # do nothing
                procssed_frame = frame
            if self.__myState==1:   # running average
                procssed_frame = self.running_average(frame)
            elif self.__myState==-1:
                break;

            # update Output window

            procssed_frame = cv.CreateImage( ( int(size[0]*self.preview_scale) , int(size[1]*self.preview_scale)), frame.depth, frame.nChannels)
            cv.Resize(frame, procssed_frame)
            if is_rect_nonzero(self.selection):
                roi_x,roi_y,roi_width,roi_height=self.selection
            else:
                roi_x,roi_y,roi_width,roi_height=0,0,procssed_frame.width,procssed_frame.height

            cropped = cv.CreateImage((roi_width, roi_height), procssed_frame.depth, procssed_frame.nChannels)
            src_region = cv.GetSubRect(procssed_frame, (roi_x, roi_y, roi_width, roi_height))
            cv.Copy(src_region, cropped)            

            cv.ShowImage( "fmTracking_Output_Window", cropped)


            # Handle Key inputs
            c = cv.WaitKey(1) % 0x100
            if c<> 0xff:
                self.on_msgArrival(chr(c))


if __name__=="__main__":

    demo = fmTrackingDemo()
#    demo.set_capture_resolution(1280,720)
    demo.set_capture_file('fm_tracking_pattern_01.jpg')
    demo.run()
    cv.DestroyAllWindows()
